clc,clear,close all

pkg load signal

addpath("./system_idnetification");

## load data
file_name = 'ulog_rate_chirp_sweep_result.mat';
load(file_name);


## sampling Time and freq
ts = median(diff(t));
fs = 1/ts;
printf("Sample frequencies: %d\n", fs);

## set options
# freq range to be shown in frequency response
freq_range = [0.01 50];

## tfestimate options
freq_point = fs;
window = hanning(4*int16(fs));
overlap_per = 0.5;
Nfft = 2^15;

## delete input and output mean
y = y - mean(y);
u = u - mean(u);

## frequency domain response.
[T_uy, F_uy] = tfestimate(u, y, window, overlap_per, Nfft, fs);

##T_uyf_unwarped = unwrap(angle(T_uyf));
T_uy_unwarped = unwrap(angle(T_uy));

plant_id = tfest(u', y', 2, 1, 1/250);

figure(1);
subplot(2,1,1);
semilogx(F_uy, 20*log10(abs(T_uy)));
xlim(freq_range);
subplot(2,1,2);
semilogx(F_uy, rad2deg(T_uy_unwarped));
hold on;

figure(2);
bode(plant_id);


